#include "pfsm_state.h"
#include "axisgroup_robot.h"
#include "log/zuclog.h"

#include "pfsm_free.h"
#include "pfsm_servo.h"
#include "pfsm_drag.h"
#include "pfsm_coord.h"

#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"

using namespace axisgroup;

#define PFSM_INFO(fmt, ...) zuclog_info("PFSM", fmt, ##__VA_ARGS__)
#define PFSM_ERR(fmt, ...) zuclog_error("PFSM", fmt, ##__VA_ARGS__)
#define PFSM_DEBUG(fmt, ...) zuclog_debug("PFSM", fmt, ##__VA_ARGS__)

std::map<PFsmState::PlanMode, std::string> axisgroup::PFsmState::PLAN_MODE_NAME = {{{PlanMode::PLAN_FREE, "FREE"},
                                                                                    {PlanMode::PLAN_SERVO, "SERVO"},
                                                                                    {PlanMode::PLAN_DRAG, "DRAG"},
                                                                                    {PlanMode::PLAN_COORD, "COORD"},
                                                                                    {PlanMode::PLAN_IDLE, "IDLE"}}};

axisgroup::PFsmState::PFsmState(Robot* grp) : grp_(grp) {}

int axisgroup::PFsmState::init() { return 0; }

int axisgroup::PFsmState::enter_mode()
{
    int res = enter();
    if (!res)
    {
        status_.errcode = 0;
        status_.status = PlanMovingStatus::MOVING_STOPED;
    }
    PFSM_INFO("Enter %s Mode!", PLAN_MODE_NAME[plan_type_].c_str());
    return 0;
}

// 退出为切模式，切模式则一定为stanby状态
void axisgroup::PFsmState::exit_mode()
{
    exit();
    status_.errcode = 0;
    status_.status = PlanMovingStatus::MOVING_STOPED;
}

PlanStatus axisgroup::PFsmState::update()
{
    status_.errcode = update_plan();
    status_.status = check_inpos();
    inpos_ = status_.status == PlanMovingStatus::MOVING_STOPED;
    return status_;
}

void axisgroup::PFsmState::pause() { is_paused_ = true; }

void axisgroup::PFsmState::resume() { is_paused_ = false; }

void axisgroup::PFsmState::CAT2_stop()
{
    abort();
    is_paused_ = false;
    status_.status = PlanMovingStatus::MOVING_ERRSTOPPING;
}

void axisgroup::PFsmState::abort_plan()
{
    abort();
    is_paused_ = false;
    status_.status = PlanMovingStatus::MOVING_STOPPING;
}

PlanMovingStatus axisgroup::PFsmState::check_inpos()
{
    if (is_fdb_inpos())
    {
        if (status_.status == PlanMovingStatus::MOVING_ERRSTOPPING)
        {
            return PlanMovingStatus::MOVING_STOPED;
        }
        else
        {
            return PlanMovingStatus::MOVING_STOPED;
        }
    }
    else
    {
        if (!is_stopping())
        {
            return PlanMovingStatus::MOVING_RUNNING;
        }
        else
        {
            return PlanMovingStatus::MOVING_STOPPING;
        }
    }
    return status_.status;
}

bool axisgroup::PFsmState::is_fdb_inpos()
{
    if (!is_plan_over())
    {
        return false;
    }
    else
    {
        // #TODO 判断反馈到位，或到位超时
    }
    return true;
}

axisgroup::PFsmIdle::PFsmIdle(Robot* grp) : PFsmState(grp) {}

int axisgroup::PFsmIdle::enter() { return 0; }

int axisgroup::PFsmIdle::update_plan()
{
    // 下使能跟随反馈，上使能保持不动
    if (!grp_->cmd_.enable)
    {
        grp_->cmd_rst_.jpos = grp_->get_moving_state().fdb.fdb_pos;
    }
    else {}
    grp_->kine_->fkine(grp_->cmd_rst_.jpos,
                       grp_->cmd_rst_.pose,
                       &(RobotToolMgr::get_rob_tool(grp_->tool_id_).pose()),
                       &(UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_)));
    return 0;
}

std::shared_ptr<PFsmState> axisgroup::creat_pfsm(Robot* grp, PFsmState::PlanMode mode)
{
    std::shared_ptr<PFsmState> res;
    switch (mode)
    {
    case PFsmState::PlanMode::PLAN_FREE:
        res = std::make_shared<PFsmFree>(grp);
        break;
    case PFsmState::PlanMode::PLAN_SERVO:
        res = std::make_shared<PFsmServo>(grp);
        break;
    case PFsmState::PlanMode::PLAN_DRAG:
        res = std::make_shared<PFsmDrag>(grp);
        break;
    case PFsmState::PlanMode::PLAN_COORD:
        res = std::make_shared<PFsmCoord>(grp);
        break;
    case PFsmState::PlanMode::PLAN_IDLE:
        res = std::make_shared<PFsmIdle>(grp);
        break;
    default:
        return nullptr;
    }
    res->plan_type_ = mode;
    return res;
}
